/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.iaroc.irobot.sample;

import com.iaroc.irobot.*;
import com.iaroc.irobot.util.XLights;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.LEDColor;
import com.sun.spot.util.*;
import java.util.Vector;


/**
 *
 * @author Mark
 */
public class WallSonarEventHandler extends IRobotCreateEventHandler
    {
    IRobotCreate create;
    EDemoBoard board;
    MaxSonarEZ1 sonar;
    XLights lights;
    int createVelocity;
    int totalDistance;
    int smallSpike;//resets when there is a big number
    int bigSpike;//resets when there is a small number
    boolean bigCheck;
    boolean smallCheck;
    boolean whichCheck;//if its true you make sure the numbers are all small
    //and react when there is a small reading (if small spike is > 3)
    //if its false you check for big numbers and react when there are big numbers
    // (if big spike is > 3)
    final int HALF_NOTE = 64;
    final int QUARTER_NOTE = 32;
    final int EIGHT_NOTE = 16;
    final int SIX_NOTE = 8;

    final int[] notes = {
        55, QUARTER_NOTE, //G
        55, QUARTER_NOTE, //G
        55, QUARTER_NOTE, //G
        51, 24, // E flat
        58, SIX_NOTE, // B flat
        55, QUARTER_NOTE, //G        
        51, 24, // E flat
        58, SIX_NOTE, // B flat
        55, HALF_NOTE, //G 
        62, QUARTER_NOTE, // D
        62, QUARTER_NOTE, // D
        62, QUARTER_NOTE, // D
        63, 24, // e flat
        58, SIX_NOTE, // B flat
        53, QUARTER_NOTE, //F
        48, 24, //F
        58, SIX_NOTE, //B flat
        55, HALF_NOTE, //G
        67, QUARTER_NOTE, //HIGH G
        55, 24, //G
        55, QUARTER_NOTE, //G
        66, 24, //F SHARP
        65, SIX_NOTE, //F
        64, SIX_NOTE, //E
        65, SIX_NOTE, //F
        65, EIGHT_NOTE, //F
        127, 32,
        127, 16,
        127, 16,
        127, 8,
        127,8,
        127, 8,
        127, 2, 
        127, 2, 
        127, 2, 
        127, 2, 
        };
    
    final int[] nonotes = new int[] {
        55, 32 // G
        , 55, 32 // G
        , 55, 32 // G
        , 51, 16 // D#
        , 58, 16 // A#
        , 55, 32 // G
        , 51, 16 // D#
        , 58, 16 // A#
        , 55, 32 // G
        , 62, 32 // D
        , 62, 32 // D
        , 62, 32 // D
        , 63, 16 // D#
        , 58, 16 // A#
        , 54, 32 // F#
        , 51, 16 // D#
        , 58, 16 // A#
        , 55, 32 // G
    };
    
    public int IDEAL_CLOSENESS = 32;
    public int IDEAL_FARNESS = 28;
    public boolean first = true;
    public static final int IS_HALLWAY = 40;
    public static final int LEFT_LED = 0;
    public static final int MID_LEFT_LED = 1;
    public static final int MID_RIGHT_LED = 6;
    public static final int RIGHT_LED = 7;
    
    public WallSonarEventHandler()
        {
        super();
        board = EDemoBoard.getInstance();
        sonar = new MaxSonarEZ1(EDemoBoard.A0);
        lights = new XLights(board, 2, 5);
        lights.startPsilon();
        create = new IRobotCreate();
        lights.setColor(LEDColor.WHITE);
        create.song(2, notes);
//        create.playSong(2);
        createVelocity = 500;
        board.getLEDs()[LEFT_LED].setColor(LEDColor.BLUE);
        board.getLEDs()[MID_LEFT_LED].setColor(LEDColor.RED);
        board.getLEDs()[MID_RIGHT_LED].setColor(LEDColor.GREEN);
        bigCheck = false;
        smallCheck = false;
        whichCheck = sonar.getDistance() < IDEAL_FARNESS;
        }
    
    public void pollSensors()
        {
        create.sensors(IRobotCreateConstants.SENSORS_GROUP_ID6, this);
        wallSonarEvent(sonar.getDistance());
        }
    
    public void wallSonarEvent(int sonarSignal)
        {
        super.wallSignalEvent(0, sonarSignal);
        System.out.println("Wall Sonar signal is : " + sonarSignal);
        smallSpike = sonarSignal < IDEAL_FARNESS? smallSpike + 1: 0;
        //resets when there is a big reading
        bigSpike = sonarSignal > IDEAL_FARNESS? bigSpike + 1: 0;
        //resets when there is a small reading
        if (whichCheck)
            {
            if (bigSpike > 4)
                create.driveDirect(400, 500);
            else
                create.drive(createVelocity, -150);
            }
        else
            {
            if (smallSpike > 4)
                create.drive(createVelocity, -150);
            else
                create.driveDirect(400, 500);
            }
        if (smallSpike > 4 || bigSpike > 4)
            whichCheck = sonar.getDistance() < IDEAL_FARNESS;
//        if (spikeOccurences > 3)
//            {
//            if (sonarSignal > IDEAL_FARNESS)
//                {
//                System.out.println("Making Right Turn");
//                create.drive(createVelocity, -150);
//                }
//            else
//                {
//                setHighAndLow();
//                create.drive(createVelocity, -200);
//                }
//            }
//        else
//            create.drive(createVelocity, -200);
        }

    public void setHighAndLow()
        {
        int ret = 0;
        for (int i = 0; i < 5; i++)
            {
            ret += sonar.getDistance();
            Utils.sleep(50);
            }
        ret = ret/5;
        IDEAL_CLOSENESS = ret - 10;
        IDEAL_FARNESS = ret + 10;
        if (first)
            create.driveDirect(300, 500);
        }
    
    public void bumpLeftEvent(boolean oldBoolean, boolean bumpLeft) 
        {
        super.bumpLeftEvent(oldBoolean, bumpLeft);
        if (bumpLeft)
            {
            System.out.println("left was bumped now turning");
            create.spinLeft();
            create.waitAngle(55);
            create.stop();
            }
        System.out.println("Done turning");
        }

    public void bumpRightEvent(boolean oldBoolean, boolean bumpRight) 
        {
        super.bumpRightEvent(oldBoolean, bumpRight);
        if (bumpRight)
            {
            System.out.println("right was bumped now turning");
            create.spinLeft();
            create.waitAngle(3);
            create.stop();
            }
        System.out.println("Done turning");
        }

    public void songPlayingEvent(boolean oldBoolean, boolean songPlaying)
        {
        super.songPlayingEvent(oldBoolean, songPlaying);
        if (!songPlaying)
            create.playSong(2);
        }

    
    
    public void eventBatchStart() 
        {
        super.eventBatchStart();
        }
    
    public void eventBatchEnd() 
        {
        super.eventBatchEnd();
        }
    
    
    }
